/*
 * To change this template, choose Tools | Templates
 * and open the template in the editor.
 */
package ai;

import Jama.Matrix;
import control.Angvel;
import control.Kalman;
import control.PDController;
import control.Shoot;
import control.Speed;
import control.TankAction;
import control.Utility;
import java.util.ArrayList;
import java.util.Date;
import java.util.List;
import java.util.Random;
import java.util.Stack;
import plot.GNUPlot;
import world.Base;
import world.Enemy;
import world.Flag;
import world.Friend;
import world.Point;

/**
 *
 * @author dheath
 */
public class TourneyState
{

    protected final double shotRange = 346.0;
    protected final double bulletSpeed = 100.0;

    public void nextMove(Tournament bot)
    {
    }

    protected Enemy getBestEnemy(Friend me, Tournament bot)
    {
        Enemy e = null;
        double shortestDist = 1000.0;
        for (Enemy ee : bot.getWorld().getEnemies())
        {
            if (!ee.getStatus().equalsIgnoreCase("dead"))
            {
                double dist = Utility.distance(me.getLocation(), ee.getLocation());
                if (dist < shortestDist)
                {
                    e = ee;
                    shortestDist = dist;
                }
            }
        }
        return e;
    }

    protected double getPredictingTime(Point enemy, Point me, Point enemyV, double dT)
    {
        //(s^2-(v.v))*t^2 - (2*((A-B).v))*t - (A-B).(A-B) = 0
        Point temp = Utility.minus(enemy, me);

        double a = (bulletSpeed) * (bulletSpeed) - Utility.dot(enemyV, enemyV);
        double b = -2 * (Utility.dot(temp, enemyV));
        double c = -1 * Utility.dot(temp, temp);

        //System.out.println("a,b,c: "+a+" "+b+" "+c);
        return Utility.root(a, b, c);
    }

    protected Point getFuturePosition(double dT, Kalman filter)
    {
        Matrix Ut = filter.getFuturePrediction(dT);

        return new Point(Ut.get(0, 0), Ut.get(3, 0));
    }

    protected Point[] getEstimatedPosition(List<Point> buffer, Tournament bot, double dT, Kalman filter)
    {

        double vOldx = (buffer.get(1).getX() - buffer.get(0).getX()) / dT;
        double vOldy = (buffer.get(1).getY() - buffer.get(0).getY()) / dT;

        double vNewx = (buffer.get(2).getX() - buffer.get(1).getX()) / dT;
        double vNewy = (buffer.get(2).getY() - buffer.get(1).getY()) / dT;

        double accx = (vNewx - vOldx) / dT;
        double accy = (vNewy - vOldy) / dT;

        double[][] zz = {{buffer.get(2).getX()}, {vNewx}, {accx}, {buffer.get(2).getY()}, {vNewy}, {accy}};
        Matrix Zt = new Matrix(zz);

        Matrix Ut = filter.updateFilter(Zt, dT);


        Point[] res = new Point[2];
        res[0] = new Point(Ut.get(0, 0), Ut.get(3, 0));
        res[1] = new Point(Ut.get(1, 0), Ut.get(4, 0));

        return res;

    }
}

//Intitial state for aggressive tanks
class InitialAttack extends TourneyState
{

    private final double distance = 50;
    private final long mstotravel = 6000;
    private final Point north = new Point(0, 400);
    private final Point south = new Point(0, -400);
    private final Point west = new Point(-400, 0);
    private final Point east = new Point(400, 0);
    private long initTime;
    private double angle;
    private Point destPoint;
    private long seed;
    private boolean linedUp;
    //  private PDController pd;
    public InitialAttack()
    {
        this.initTime = 0;//(new Date()).getTime();
        this.destPoint = null;
//        pd = new PDController();
        this.linedUp = false;
        Random r = new Random();
        seed = r.nextInt();
        System.out.println("InitialAttack!");
    }

    public void nextMove(Tournament bot)
    {
        Friend me = bot.getWorld().getFriends().get(bot.getBotId());

        if (initTime == 0)
        {
            Random gen = new Random(seed);
            Random gen2 = new Random();
            initTime = (new Date()).getTime();
            if (Utility.distance(me.getLocation(), north) < 50)
            {
                double dY = gen.nextDouble() * -40 - 10;
                double dX = gen2.nextDouble() * 300 - 150;
                destPoint = Utility.plus(north, new Point(dX, dY));
            } else
            {
                if (Utility.distance(me.getLocation(), south) < 50)
                {
                    double dY = gen.nextDouble() * 40 + 10;
                    double dX = gen2.nextDouble() * 300 - 150;
                    destPoint = Utility.plus(south, new Point(dX, dY));
                } else
                {
                    if (Utility.distance(me.getLocation(), east) < 50)
                    {
                        double dX = gen.nextDouble() * -40 - 10;
                        double dY = gen2.nextDouble() * 300 - 150;
                        destPoint = Utility.plus(east, new Point(dX, dY));
                    } else
                    {
                        if (Utility.distance(me.getLocation(), west) < 50)
                        {
                            double dX = gen.nextDouble() * 40 + 10;
                            double dY = (gen2.nextDouble() * 300) - 150;
                            destPoint = Utility.plus(north, new Point(dX, dY));
                        } else
                        {
                            destPoint = me.getLocation();
                        }
                    }
                }
            }

        }

        //Transition -> HuntEnemy
        //once you reach past a certain distance
        //or enemy tank is near
        if (this.initTime + mstotravel < (new Date()).getTime()) //(me.getVx()==0) ^ (me.getVy()==0) )// transition condition
        {
            if (bot.isIsAggressive())
            {
                bot.setState(new HuntEnemy(bot.getNoise()));
            } else
            {
                (new Speed(0.0)).performAction(bot.getMessenger(), bot.getBotId());
                bot.setState(new Turret(bot.getNoise()));
            }
            return;
        }

        //randomly shoot off in whatever direction(away from home base)
        // pick a direction
        Point loc = me.getLocation();
        
        if (linedUp)
        {

            (new Speed(1.0)).performAction(bot.getMessenger(), bot.getBotId());

        } else
        {

            Date now = new Date();
            long dT = now.getTime() - bot.getLastTime().getTime();
            bot.setLastTime(now);

            //System.out.println("change in Time: " + dT);

            List<TankAction> actions = PDController.move(me.getAngle(), me.getLocation(), destPoint, dT);
            for (TankAction a : actions)
            {
                if (a instanceof Angvel)
                {
                    if (a.getError() < .3)// 10 degrees
                    {
                        linedUp = true;
                        (new Angvel(0.0)).performAction(bot.getMessenger(), bot.getBotId());
                        break;
                    }
                    a.performAction(bot.getMessenger(), bot.getBotId());
                }

            }
            (new Speed(0.0)).performAction(bot.getMessenger(), bot.getBotId());
        }
    }
}

class HuntEnemy extends TourneyState
{

    
    Kalman enemyLocation;
    Kalman enemyVel; //test
    Kalman myLocation;
    List<Point> bufferE;
    List<Point> bufferMe;
    List<Point> bufferV;
    private Enemy lastEnemy;
    private int countP = 0;

    public HuntEnemy(double noise)
    {
        enemyLocation = new Kalman(noise);
        enemyVel = new Kalman(noise);
        myLocation = new Kalman(noise);

        bufferE = new ArrayList();
        bufferMe = new ArrayList();
        bufferV = new ArrayList();

        System.out.println("Hunt Enemy!!!");
    }

    public void nextMove(Tournament bot)
    {

        //Transition -> SearchFlag
        //all enemies are dead
        //or time limit

        //Transition -> Patrol
        //no more enemies in range of home base

        Date now = new Date();
        if (now.getTime() - bot.getGameTime().getTime() > 60000)
        {
            bot.setState(new SearchFlag(true));
        }
        //searches for and Hunts enemies
        int maxPlot = 30;

        Friend me = bot.getWorld().getFriends().get(bot.getBotId());

        Enemy e = getBestEnemy(me, bot);


        if (e == null)
        {
            bot.setState(new SearchFlag());
            return;
        /*e = bot.getWorld().getEnemies().get(0);
        enemyLocation.resetFilter(noise);
        enemyVel.resetFilter(noise);
        lastEnemy = 0;*/
        }
        if (countP % 25 == 0)
        {
            myLocation.resetFilter(bot.getNoise());
            enemyLocation.resetFilter(bot.getNoise());
            enemyVel.resetFilter(bot.getNoise());
        }


        double dT = (now.getTime() - bot.getLastTime().getTime()) / 1000.0;
        //System.out.println("Time from last time: "+dT);
        bot.setLastTime(now);


        bufferE.add(e.getLocation());
        bufferMe.add(me.getLocation());

        if (bufferE.size() > 2)
        {
            Point[] enInfo = getEstimatedPosition(bufferE, bot, dT, enemyLocation);

            Point enPos = enInfo[0];

            Point pme = getEstimatedPosition(bufferMe, bot, dT, myLocation)[0];



            Point ev = enInfo[1];//new Point(enInfo[1].getX()*10, enInfo[1].getY()*10);
            bufferV.add(ev);
            if (bufferV.size() > 2)
            {
                ev = getEstimatedPosition(bufferV, bot, dT, enemyVel)[0];
                bufferV.remove(0);
            }

            double shotSec = getPredictingTime(enPos, pme, ev, dT);

            Point en = getFuturePosition(shotSec, enemyLocation);


            List<TankAction> moves = PDController.move(me.getAngle(), pme, en, (long) (dT * 1000.0), .2);

            for (TankAction a : moves)
            {

                a.performAction(bot.getMessenger(), bot.getBotId());

            }
            if (Utility.distance(pme, en) < 25)
            {
                (new Shoot()).performAction(bot.getMessenger(), bot.getBotId());
            }


            bufferE.remove(0);
            bufferMe.remove(0);
        }
    }
}

class Turret extends TourneyState
{

    Kalman enemyLocation;
    Kalman enemyVel; //test
    Kalman myLocation;
    List<Point> bufferE;
    List<Point> bufferMe;
    List<Point> bufferV;
    private int lastEnemy;
    private int countP = 0;

    public Turret(double noise)
    {
        enemyLocation = new Kalman(noise);
        enemyVel = new Kalman(noise);
        myLocation = new Kalman(noise);

        bufferE = new ArrayList();
        bufferMe = new ArrayList();
        bufferV = new ArrayList();

        lastEnemy = 0;

        System.out.println("Turret!");
    }

    public void nextMove(Tournament bot)
    {

        //Transistion -> getFlag
        //Time limit
        //All enemies are dead and all friends are dead
        Date now = new Date();
        if (now.getTime() - bot.getGameTime().getTime() > 180000)
        {
            bot.setState(new SearchFlag());
        }

        //Stationary and shoots enemies
        //(already implemented)
        int maxPlot = 30;

        Friend me = bot.getWorld().getFriends().get(bot.getBotId());

        Enemy e = getBestEnemy(me, bot);


        if (e == null)
        {
            e = bot.getWorld().getEnemies().get(0);
            enemyLocation.resetFilter(bot.getNoise());
            enemyVel.resetFilter(bot.getNoise());
            lastEnemy = 0;
        }
        if (countP % 25 == 0)
        {
            myLocation.resetFilter(bot.getNoise());
            enemyLocation.resetFilter(bot.getNoise());
            enemyVel.resetFilter(bot.getNoise());
        }


        double dT = (now.getTime() - bot.getLastTime().getTime()) / 1000.0;
        System.out.println("Time from last time: " + dT);
        bot.setLastTime(now);


        bufferE.add(e.getLocation());
        bufferMe.add(me.getLocation());

        if (bufferE.size() > 2)
        {
            Point[] enInfo = getEstimatedPosition(bufferE, bot, dT, enemyLocation);

            Point enPos = enInfo[0];

            Point pme = getEstimatedPosition(bufferMe, bot, dT, myLocation)[0];



            Point ev = enInfo[1];//new Point(enInfo[1].getX()*10, enInfo[1].getY()*10);
            bufferV.add(ev);
            if (bufferV.size() > 2)
            {
                ev = getEstimatedPosition(bufferV, bot, dT, enemyVel)[0];
                bufferV.remove(0);
            }

            double shotSec = getPredictingTime(enPos, pme, ev, dT);

            Point en = getFuturePosition(shotSec, enemyLocation);


            List<TankAction> moves = PDController.move(me.getAngle(), pme, en, (long) (dT * 1000.0));

            for (TankAction a : moves)
            {
                if (!(a instanceof Speed))
                {
                    a.performAction(bot.getMessenger(), bot.getBotId());
                }
            }
            if (Utility.distance(pme, en) < 25)
            {
                (new Shoot()).performAction(bot.getMessenger(), bot.getBotId());
            }

            if (pme.getX() < -390 || pme.getX() > 390 || pme.getY() < -390 || pme.getY() > 390)
            {
                (new Speed(-1.0)).performAction(bot.getMessenger(), bot.getBotId());
                (new Angvel(-.3)).performAction(bot.getMessenger(), bot.getBotId());
                (new Speed(0)).performAction(bot.getMessenger(), bot.getBotId());
            }
            bufferE.remove(0);
            bufferMe.remove(0);
        }



    }
}

class Patrol extends TourneyState
{

    private Date lastTime;
    private double baseAngle;
    private boolean turning;
    private final double patrolRadius = .33 * shotRange;

    public Patrol()
    {
        baseAngle = 0.0;
        turning = true;
        lastTime = new Date();
        System.out.println("On Patrol!!");
    }

    public void nextMove(Tournament bot)
    {
        Friend me = bot.getWorld().getFriends().get(bot.getBotId());
        //Transistion -> Hunt Enemy
        //Enemy is within range
        Flag ff = bot.getWorld().getOurFlag(me);

        boolean allDead = true;
        for (Enemy e : bot.getWorld().getEnemies())
        {
            if (!e.getStatus().equalsIgnoreCase("dead"))
            {
                allDead = false;
                if (Utility.distance(e.getLocation(), ff.getLocation()) < shotRange)
                {
                    bot.setState(new HuntEnemy(bot.getNoise()));
                    return;
                }
            }
        }
        if (allDead)
        {
            bot.setState(new SearchFlag());
        }

        //Transistion -> SearchFlag
        //All enemies are dead
        //time limit

        //Patrols an area for enemies
        // (wild duck heuristic)
        Date now = new Date();
        System.out.println("time delay: " + (now.getTime() - lastTime.getTime()));
        lastTime = now;

        Random rand = new Random();
        double factor = 1.0;
        if (rand.nextBoolean())
        {
            factor = -1.0;
        }


        if (baseAngle == 0.0)
        {
            (new Speed(.4 + rand.nextDouble())).performAction(bot.getMessenger(), bot.getBotId());
            (new Angvel((.2 + rand.nextDouble()) * factor)).performAction(bot.getMessenger(), bot.getBotId());
        }

        if (turning)
        {
            if (Math.abs(baseAngle - me.getAngle()) >= .9)
            {
                turning = false;
                (new Angvel((0.2 + rand.nextDouble()))).performAction(bot.getMessenger(), bot.getBotId());
                (new Speed(.8 + rand.nextDouble())).performAction(bot.getMessenger(), bot.getBotId());
            } else
            {
                (new Angvel(0.7)).performAction(bot.getMessenger(), bot.getBotId());
            }
        } else
        {

            //	double wall = 800.0 / 2.0 - 6.0;
            //	double currAng = Utility.getRadians02PI(me.getAngle());
            /*if(  // if tank is pointed in direction of wall he is against
            (          0 < currAng &&       currAng <= Math.PI &&  me.getLocation().getY() > wall)      ||
            (Math.PI / 2 < currAng && currAng <= 3 * Math.PI/2 &&  me.getLocation().getX() < -1 * wall) ||
            (    Math.PI < currAng && currAng <=   2 * Math.PI &&  me.getLocation().getY() < -1 * wall) ||
            ( (currAng >= 3*Math.PI/2 || currAng <= Math.PI/2) &&  me.getLocation().getX() > wall)
            )
             */
            Flag myFlag = null;
            List<Flag> flagList = bot.getWorld().getFlags();
            for (Flag f : flagList)
            {
                if (f.getOwnerColor().equalsIgnoreCase(me.getColor()))
                {
                    myFlag = f;
                    break;
                }
            }
            if (Utility.distance(me.getLocation(), myFlag.getLocation()) > patrolRadius)
            {
                (new Angvel(1.0)).performAction(bot.getMessenger(), bot.getBotId());
                (new Speed(-.1)).performAction(bot.getMessenger(), bot.getBotId());
                turning = true;
                baseAngle = me.getAngle();
            } else
            {
                (new Speed(.8 + rand.nextDouble())).performAction(bot.getMessenger(), bot.getBotId());
                (new Angvel((.2 + rand.nextDouble()) * factor)).performAction(bot.getMessenger(), bot.getBotId());
            }
        }
    }
}

class SearchFlag extends TourneyState
{

    private boolean havePath;
    private PDController controller;
    private Stack<Point> path;
    private Point currentPoint;
    private boolean reverse;
    
    public SearchFlag()
    {
        havePath = false;
        controller = new PDController();
        reverse = false;
        System.out.println("Search Flag!!!!");
    }
    
    public SearchFlag(boolean reverse)
    {
        havePath = false;
        controller = new PDController();
        this.reverse = reverse;

        System.out.println("Search Flag!!!!");
    }
    
    public void nextMove(Tournament bot)
    {
        //Transition -> GoHomeWithFlag
        //When the flag is captured 
        boolean hasFlag = false;
        for (Friend f : bot.getWorld().getFriends())
        {
            if (!f.getFlag().equalsIgnoreCase("none"))
            {
                hasFlag = true;
                bot.setState(new GoHomeWithFlag(reverse));
                return;
            }
        }

        //finds path to flag and gets it

        //(already implemented)
        Friend me = bot.getWorld().getFriends().get(bot.getBotId());

        if (!havePath)
        {
            Flag end = null;
            for (Flag flag : bot.getWorld().getFlags())
            {
                if (!flag.getOwnerColor().equalsIgnoreCase(me.getColor()))
                {
                    end = flag;
                }
            }
            GNUPlot plot = new GNUPlot(bot.getWorld(), false, true, "multiagent.plt", .00000000001);

            path = bot.getSearcher().search(me.getLocation(), end.getLocation(), bot.getWorld(), plot);//PDController.setPath()
            currentPoint = path.pop();

            plot.closeConnection();
            havePath = true;
        } else
        {

            if (!path.empty())
            {
                if (Utility.distance(me.getLocation(), currentPoint) < 50)
                {
                    currentPoint = path.pop();

                }
            }

            Date now = new Date();
            long dT = now.getTime() - bot.getLastTime().getTime();
            bot.setLastTime(now);

            //System.out.println("change in Time: " + dT);

            List<TankAction> actions;
            
            if(reverse)
            {
                actions = controller.reverse(me.getAngle(), me.getLocation(), currentPoint, dT, .1);
            }
            else
            {
                actions = controller.move(me.getAngle(), me.getLocation(), currentPoint, dT, .1); 
            }

            for (TankAction action : actions)
            {

                action.performAction(bot.getMessenger(), me.getBotId());

            }
        }

    }
}

class GoHomeWithFlag extends TourneyState
{

    private boolean havePath;
    private PDController controller;
    private Stack<Point> path;
    private Point currentPoint;
    
    private boolean reverse;
    
    public GoHomeWithFlag(boolean reverse)
    {
        System.out.println("GO HOME NOW!");
        havePath = false;
        controller = new PDController();
        this.reverse = reverse;
    }
    public GoHomeWithFlag()
    {
        System.out.println("GO HOME NOW!");
        havePath = false;
        controller = new PDController();
        reverse = false;
    }

    public void nextMove(Tournament bot)
    {
        //Transition -> SearchFlag
        //Flag is droped
       /* boolean hasFlag = false;
        for (Friend f : bot.getWorld().getFriends())
        {
        if (!f.getFlag().equalsIgnoreCase("none"))
        {
        hasFlag = true;
        break;
        }
        }
        if (!hasFlag)
        {
        bot.setState(new SearchFlag());
        }*/

        //returns home (already implemented)
        Friend me = bot.getWorld().getFriends().get(bot.getBotId());

        if (!havePath)
        {
            Base end = null;
            for (Base base : bot.getWorld().getBases())
            {
                if (base.getTeamColor().equalsIgnoreCase(me.getColor()))
                {
                    end = base;
                    break;
                }
            }
            GNUPlot plot = new GNUPlot(bot.getWorld(), false, true, "multiagent.plt", .00000000001);

            path = bot.getSearcher().search(me.getLocation(), end.getLocation(), bot.getWorld(), plot);//PDController.setPath()
            currentPoint = path.pop();

            plot.closeConnection();
            havePath = true;
        } else
        {

            if (!path.empty())
            {
                if (Utility.distance(me.getLocation(), currentPoint) < 50)
                {
                    currentPoint = path.pop();
                //System.out.println("new point: " + currentPoint);
                }
            }

            Date now = new Date();
            long dT = now.getTime() - bot.getLastTime().getTime();
            bot.setLastTime(now);

            //System.out.println("change in Time: " + dT);

            List<TankAction> actions;
            
            if(reverse)
            {
                actions = controller.reverse(me.getAngle(), me.getLocation(), currentPoint, dT, .1);
            }
            else
            {
                actions = controller.move(me.getAngle(), me.getLocation(), currentPoint, dT, .1); 
            }

            for (TankAction action : actions)
            {
                action.performAction(bot.getMessenger(), me.getBotId());
            }
        }

    }
}

